﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Diagnostics;

namespace FunkAIChallenge
{
    /// <summary>
    /// Simple PlayerController example that implements functionality of the test controller 
    /// in the original python source.
    /// </summary>
    public class FunkTestController : PlayerController
    {
        /// <summary>
        /// Construct with the provided player interface.
        /// </summary>
        public FunkTestController(PlayerInterface iface) 
            : base(iface) 
        {}

        /// <summary>
        /// Perform one controller step. Typically consisting of reading world and player arm state
        /// from the player interface and updating the torques applied at each player arm joint.
        /// </summary>
        public override void Step()
        {
            float t = _iface.CurrentTime;
            _iface.SetJointTorque(0, (float)Math.Sin(t) * _iface.MaxTorqueArray[0]);
            _iface.SetJointTorque(1, (float)Math.Sin(t*1.7f) * _iface.MaxTorqueArray[1]);
            _iface.SetJointTorque(2, (float)Math.Sin(t*2.3f) * _iface.MaxTorqueArray[2]);
        }
    }
}
